{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# Similarity3\n",
        "\n",
        "A `Similarity3` represents a similarity transformation in 3D space, which is a combination of a rotation, a translation, and a uniform scaling. It is an element of the special similarity group Sim(3), which is also a Lie group. Its 2-dimensional analog is `Similarity2`. It is included in the top-level `gtsam` package."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Similarity3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop \n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import Similarity3, Rot3, Point3, Pose3\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization and Properties\n",
        "\n",
        "A `Similarity3` can be initialized in several ways:\n",
        "- With no arguments to create an identity transform `(R=I, t=0, s=1)`.\n",
        "- With a `Rot3` for rotation, a `Point3` for translation, and a `float` for scale."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Identity:\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0 s: 1\n",
            "\n",
            "S1:\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 10 20 30 s: 2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Identity transform\n",
        "s_identity = gtsam.Similarity3()\n",
        "print(f\"Identity:\\n{s_identity}\")\n",
        "\n",
        "# Transform with 30-degree yaw, translation (10, 20, 30), and scale 2\n",
        "R = Rot3.Yaw(np.pi / 6)\n",
        "t = Point3(10, 20, 30)\n",
        "s = 2.0\n",
        "S1 = Similarity3(R, t, s)\n",
        "print(f\"S1:\\n{S1}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The transform's properties can be accessed using `rotation()`, `translation()`, and `scale()`. The 4x4 matrix representation can be obtained with `matrix()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Rotation:\n",
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "Translation: [10. 20. 30.]\n",
            "Scale: 2.0\n",
            "Matrix:\n",
            "[[ 0.866 -0.5    0.    10.   ]\n",
            " [ 0.5    0.866  0.    20.   ]\n",
            " [ 0.     0.     1.    30.   ]\n",
            " [ 0.     0.     0.     0.5  ]]\n"
          ]
        }
      ],
      "source": [
        "print(f\"Rotation:\\n{S1.rotation()}\")\n",
        "print(f\"Translation: {S1.translation()}\")\n",
        "print(f\"Scale: {S1.scale()}\")\n",
        "print(f\"Matrix:\\n{np.round(S1.matrix(), 3)}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Mathematical Representation\n",
        "\n",
        "GTSAM's `Similarity3` implementation follows a convention similar to that described in Ethan Eade's \"Lie Groups for 2D and 3D Transformations\". An element of Sim(3) is a tuple $(R, t, s)$ where:\n",
        "$$ \n",
        "\\mathbf{R} \\in SO(3), \\mathbf{t} \\in \\mathbb{R}^3, s \\in \\mathbb{R}\n",
        "$$\n",
        "\n",
        "It can be represented by a $4 \\times 4$ matrix:\n",
        "$$ \n",
        "T = \\begin{pmatrix} \\mathbf{R} & \\mathbf{t} \\\\ \\mathbf{0} & s^{-1} \\end{pmatrix} \\in \\text{Sim(3)}\n",
        "$$\n",
        "\n",
        "The composition of two transforms $T_1 = (R_1, t_1, s_1)$ and $T_2 = (R_2, t_2, s_2)$ is given by matrix multiplication:\n",
        "$$ \n",
        "T_1 \\cdot T_2 = \\begin{pmatrix} \\mathbf{R}_1 & \\mathbf{t}_1 \\\\ \\mathbf{0} & s_1^{-1} \\end{pmatrix} \\begin{pmatrix} \\mathbf{R}_2 & \\mathbf{t}_2 \\\\ \\mathbf{0} & s_2^{-1} \\end{pmatrix} = \\begin{pmatrix} \\mathbf{R}_1 \\mathbf{R}_2 & \\mathbf{R}_1 \\mathbf{t}_2 + s_2^{-1} \\mathbf{t}_1 \\\\ \\mathbf{0} & (s_1 s_2)^{-1} \\end{pmatrix}\n",
        "$$\n",
        "\n",
        "The inverse of a transform $T_1$ is:\n",
        "$$ \n",
        "T_1^{-1} = \\begin{pmatrix} \\mathbf{R}_1^T & -s_1 \\mathbf{R}_1^T \\mathbf{t}_1 \\\\ \\mathbf{0} & s_1 \\end{pmatrix}\n",
        "$$\n",
        "\n",
        "The action of a transform $T$ on a 3D point $\\mathbf{x}$ is defined for homogeneous points in $\\mathbb{RP}^3$. For a point $\\mathbf{x} = (x, y, z, w)^T$:\n",
        "$$ \n",
        "T \\cdot \\mathbf{x} = \\begin{pmatrix} \\mathbf{R} & \\mathbf{t} \\\\ \\mathbf{0} & s^{-1} \\end{pmatrix} \\cdot \\mathbf{x} = \\begin{pmatrix} \\mathbf{R} (x \\ y \\ z)^T + w\\mathbf{t} \\\\ s^{-1}w \\end{pmatrix} \\thicksim \\begin{pmatrix} s(\\mathbf{R} (x \\ y \\ z)^T + w\\mathbf{t}) \\\\ w \\end{pmatrix}\n",
        "$$\n",
        "For a standard 3D point (where $w=1$), this simplifies to the action $p' = s(Rp + t)$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Operations\n",
        "\n",
        "`Similarity3` can transform `Point3` and `Pose3` objects."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "S1 * [1. 1. 1.] = [20.73205081 42.73205081 62.        ]\n",
            "S1.transformFrom(pose1) =\n",
            "R: [\n",
            "\t0.666039, -0.716453, 0.207576;\n",
            "\t0.718973, 0.69074, 0.0771696;\n",
            "\t-0.198669, 0.0978434, 0.97517\n",
            "]\n",
            "t: 23.6603 53.6603      70\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Transform a Point3\n",
        "p1 = Point3(1, 1, 1)\n",
        "p2 = S1.transformFrom(p1)\n",
        "print(f\"S1 * {p1} = {p2}\")\n",
        "\n",
        "# Transform a Pose3\n",
        "pose1 = Pose3(Rot3.RzRyRx(0.1, 0.2, 0.3), Point3(5, 5, 5))\n",
        "pose2 = S1.transformFrom(pose1)\n",
        "print(f\"S1.transformFrom(pose1) =\\n{pose2}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Lie Group Sim(3)\n",
        "\n",
        "`Similarity3` implements the Lie group operations `identity`, `inverse`, `compose`, and `between`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "S1 * S2 = \n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.433013, -0.5, -0.75;\n",
            "\t0.25, 0.866025, -0.433013;\n",
            "\t0.866025, 0, 0.5\n",
            "]\n",
            "t: 26.8301 38.1699      62 s: 1\n",
            "\n",
            "S1.inverse() = \n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.866025, 0.5, 0;\n",
            "\t-0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: -37.3205  -24.641      -60 s: 0.5\n",
            "\n",
            "S1.between(S2) = \n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.433013, 0.5, -0.75;\n",
            "\t-0.25, 0.866025, 0.433013;\n",
            "\t0.866025, 0, 0.5\n",
            "]\n",
            "t: -72.8109 -56.1122     -118 s: 0.25\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create a second transform\n",
        "S2 = Similarity3(Rot3.Pitch(-np.pi/3), Point3(5, -5, 2), 0.5)\n",
        "\n",
        "# Composition (group product)\n",
        "s_composed = S1 * S2\n",
        "print(f\"S1 * S2 = \\n{s_composed}\")\n",
        "\n",
        "# Inverse\n",
        "s_inv = S1.inverse()\n",
        "print(f\"S1.inverse() = \\n{s_inv}\")\n",
        "\n",
        "# Between (relative transform): S1.inverse() * S2\n",
        "s_between = S1.between(S2)\n",
        "print(f\"S1.between(S2) = \\n{s_between}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Lie Algebra\n",
        "\n",
        "The Lie algebra of Sim(3) is the tangent space at the identity. It is represented by a 7D vector `xi = [omega_x, omega_y, omega_z, v_x, v_y, v_z, lambda]`.\n",
        "- `(omega_x, omega_y, omega_z)` is the angular velocity.\n",
        "- `(v_x, v_y, v_z)` is the translational velocity.\n",
        "- `lambda` is the log of the rate of scale change.\n",
        "\n",
        "The `Expmap` and `Logmap` functions convert between the Lie algebra and the Lie group."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expmap(xi) =\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.935755, -0.283165, 0.210192;\n",
            "\t0.302933, 0.950581, -0.0680313;\n",
            "\t-0.18054, 0.127335, 0.97529\n",
            "]\n",
            "t:  0.31224 0.436154 0.482059 s: 1.5\n",
            "\n",
            "Logmap(S_exp) = [0.1     0.2     0.3     0.4     0.5     0.6     0.40547]\n"
          ]
        }
      ],
      "source": [
        "# Create a vector in the Lie algebra\n",
        "xi = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, np.log(1.5)])\n",
        "\n",
        "# Exponential map: from Lie algebra to group\n",
        "S_exp = Similarity3.Expmap(xi)\n",
        "print(f\"Expmap(xi) =\\n{S_exp}\")\n",
        "\n",
        "# Logarithm map: from group to Lie algebra\n",
        "xi_log = Similarity3.Logmap(S_exp)\n",
        "print(f\"Logmap(S_exp) = {np.round(xi_log, 5)}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Manifold Operations\n",
        "\n",
        "`Similarity3` is also a manifold. The `retract` operation maps a tangent vector `v` from the tangent space at a `Similarity3` `p` to a new point on the manifold. The `localCoordinates` is the inverse operation.\n",
        "\n",
        "For Lie groups in GTSAM, `retract(v)` is equivalent to `p.compose(Expmap(v))`, and `localCoordinates(q)` is `Logmap(p.inverse().compose(q))`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "localCoordinates(q) from p = [ 0.193  0.359  0.201  3.857 -9.991  7.439  0.56 ]\n",
            "p.retract(v) =\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.7243, -0.365982, 0.584334;\n",
            "\t0.49552, 0.865602, -0.0720659;\n",
            "\t-0.479426, 0.341747, 0.808307\n",
            "]\n",
            "t:  8 -5  4 s: 2.1\n",
            "\n",
            "Original q =\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t0.7243, -0.365982, 0.584334;\n",
            "\t0.49552, 0.865602, -0.0720659;\n",
            "\t-0.479426, 0.341747, 0.808307\n",
            "]\n",
            "t:  8 -5  4 s: 2.1\n",
            "\n"
          ]
        }
      ],
      "source": [
        "p = Similarity3(Rot3.RzRyRx(0.1, 0.2, 0.3), Point3(1,2,3), 1.2)\n",
        "q = Similarity3(Rot3.RzRyRx(0.4, 0.5, 0.6), Point3(8,-5,4), 2.1)\n",
        "\n",
        "# Find the tangent vector to go from p to q\n",
        "v = p.localCoordinates(q)\n",
        "print(f\"localCoordinates(q) from p = {np.round(v, 3)}\")\n",
        "\n",
        "# Move from p along v to get back to q\n",
        "q_retracted = p.retract(v)\n",
        "print(f\"p.retract(v) =\\n{q_retracted}\")\n",
        "print(f\"Original q =\\n{q}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Alignment\n",
        "\n",
        "`Similarity3` provides a static `Align` method that can compute the transformation that best aligns sets of corresponding pairs. This is useful for tasks like registering two point clouds or aligning coordinate frames."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expected wSe:\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t-1, -1.22465e-16, 0;\n",
            "\t1.22465e-16, -1, 0;\n",
            "\t-0, 0, 1\n",
            "]\n",
            "t: 2 3 5 s: 2\n",
            "\n",
            "Actual wSe from Align:\n",
            "\n",
            "\n",
            "R:\n",
            " [\n",
            "\t-1, -1.22465e-16, 0;\n",
            "\t1.22465e-16, -1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 2 3 5 s: 2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Example: Align two coordinate frames (e.g., world 'w' and egovehicle 'e')\n",
        "# using pairs of poses of objects observed in both frames.\n",
        "expected_wSe = Similarity3(Rot3.Ypr(np.pi, 0, 0), Point3(2, 3, 5), 2.0)\n",
        "\n",
        "# Create source poses (objects in egovehicle frame 'e')\n",
        "eTo1 = Pose3(Rot3(), Point3(0, 0, 0))\n",
        "eTo2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])), Point3(4, 0, 0))\n",
        "\n",
        "# Create destination poses (same objects in world frame 'w')\n",
        "wTo1 = expected_wSe.transformFrom(eTo1)\n",
        "wTo2 = expected_wSe.transformFrom(eTo2)\n",
        "\n",
        "# Create a list of corresponding pose pairs\n",
        "correspondences = [(wTo1, eTo1), (wTo2, eTo2)]\n",
        "\n",
        "# Recover the transformation wSe using Align\n",
        "actual_wSe = Similarity3.Align(correspondences)\n",
        "print(f\"Expected wSe:\\n{expected_wSe}\")\n",
        "print(f\"Actual wSe from Align:\\n{actual_wSe}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}